# modify some robomaster api
import os,sys
from platform import platform
from socket import timeout
current = os.path.dirname(os.path.realpath(__file__))
sys.path.append(current)

import time

from . import simulate_msg

# __all__ = ['SIMULATE_MSG','PHYSICAL_MSG']

EP_ROBOT = None

SIMULATE_MSG = simulate_msg.SimulateMsg()
PHYSICAL_MSG = simulate_msg.PhysicalMsg()
PLATFORM_TIMER_MANAGER = simulate_msg.PlatformTimerManager()
DRIVE_SPEED_ADJUSTER = simulate_msg.DriveSpeedAdjuster()

INIT_PORT = 41999

from robomaster import robot

class PlatformException(Exception):
    def __init__(self, msg):
        self.msg = msg
        self.platform_error_handle()
    
    def platform_error_handle(self):
        if EP_ROBOT:
            if EP_ROBOT.is_initialized:
                EP_ROBOT.close()

    def __str__(self):
        return str(self.msg)

def get_initialized_robot():
    global EP_ROBOT
    if (EP_ROBOT is None):
        try:
            EP_ROBOT = robot.Robot()
            # EP_ROBOT.initialize(conn_type="ap")
            EP_ROBOT.initialize(conn_type="sta")
            EP_ROBOT.set_robot_mode(mode=robot.CHASSIS_LEAD)
            print("[platform] robot initialize success!")
        except:
            # try:
            #     EP_ROBOT = robot.Robot()
            #     EP_ROBOT.initialize(conn_type="sta")
            #     EP_ROBOT.set_robot_mode(mode=robot.CHASSIS_LEAD)
            #     print("[platform] robot initialize success!")
            # except:
            raise PlatformException("[platform] connect error!")

    return EP_ROBOT

def initial_sync():
    sender = simulate_msg.UdpClient(port=INIT_PORT)
    reply = 'False'
    while(reply != 'True'):
        reply = sender.send_msg("? Initial End",True)
        print("[platform] initial sync status:{}".format(reply))
    sender.send_msg("quit",False)

def physical_platform_initialize(ep_robot):
    ep_robot.set_robot_mode(mode=robot.CHASSIS_LEAD)
    ep_robot.led.set_led(comp="all", r=200, g=200, b=200)
    print("=====> start physical_platform_initialize")
    # time.sleep(0.5)

    ep_chassis = ep_robot.chassis
    # action = input("init action>")
    
    # 确保各种xxx_sub 正常运行
    while(1):
        F,R,B,L = PHYSICAL_MSG.get_sensor_data_info()[:]
        # print("sensor_init =",F,B,L,R)
        if (F or B or L or R):break

    
    # 手动调一下车的位置 
    # do_action是一系列简单的封装, 包含一个print W
    # xy_speed=0.5 move_dis=0.2
    # z_speed=60 rot_dis=45
    time.sleep(0.5)
    print("=====> recover car")
    # while True:
    #     ss = input("recover_action>")
    #     if (ss=='Q' or ss=='q'):
    #         break
    #     for ch in ss:
    #         do_action(ch,ep_chassis)

    need_rot = input("angle equals to 0?(y/n):")
    if (need_rot=='y' or need_rot=='Y'):
        ss="LLrF"
        do_action(ss,ep_chassis)
    else:
        while True:
            ss = input("recover_action>")
            if (ss=='Q' or ss=='q'):
                break
            for ch in ss:
                do_action(ch,ep_chassis)
    # ss='F'
    for ch in ss:
        do_action(ch,ep_chassis)
    
    time.sleep(0.5)
    
    # 获取车的初始位置[本次定位一定在4象限，且deg in [0,90)
    PHYSICAL_MSG.location_server_reset()

    pos = PHYSICAL_MSG.query_position()
    print("init = ({:.3f},{:.3f}) deg = {:.3f}".
        format(pos[0],pos[1],pos[2]))

    # 进行 _dij_move的测试 
    # time.sleep(0.5)
    # print("=====> start _dij_move check")
    # while True:
    #     ss = input("action>")
    #     if (ss=='Q' or ss=='q'):
    #         break
    #     for ch in ss:
    #         st_pos = PHYSICAL_MSG.query_position()

    #         do_action(ch,ep_chassis)

    #         ed_pos = PHYSICAL_MSG.query_position()

    #         det_pos = [ed_pos[i]-st_pos[i] for i in range(3)]
    #         print("st = ({:.3f},{:.3f}) deg = {:.3f}".format(st_pos[0],st_pos[1],st_pos[2]))
    #         print("ed = ({:.3f},{:.3f}) deg = {:.3f}".format(ed_pos[0],ed_pos[1],ed_pos[2]))
    #         print("det = ({:.3f},{:.3f}) deg = {:.3f} act = {}".format(det_pos[0],det_pos[1],det_pos[2],ch))
    
    # print("=====> finish _dij_move check")

    # 进行 drive_speed 的测试 
    # time.sleep(0.5)
    # print("=====> start drive_speed check")
    # # ep_chassis.drive_speed(0,-0.3,0,timeout=10)
    # # input("")
    # # ep_chassis.drive_speed(-0.3,0,0,timeout=3)
    # while True:
    #     ss = input("x>")
    #     if (ss=='Q' or ss=='q'):break
    #     x = float(ss) *0.2
    #     y = float(input("y>"))*0.2
    #     t = float(input("timeout>"))
    #     ep_chassis.drive_speed(x,y,0,timeout=t)

    # flush_undefined_behavior()
    # print("=====> finish _dij_move check")

    # 进行 _dij_move的测试 
    # time.sleep(0.5)
    # print("=====> start _dij_move check")
    # while True:
    #     ss = input("action>")
    #     if (ss=='Q' or ss=='q'):
    #         break
    #     for ch in ss:
    #         st_pos = PHYSICAL_MSG.query_position()

    #         do_action(ch,ep_chassis)

    #         ed_pos = PHYSICAL_MSG.query_position()

    #         det_pos = [ed_pos[i]-st_pos[i] for i in range(3)]
    #         print("det = ({:.3f},{:.3f}) deg = {:.3f} act = {}".format(det_pos[0],det_pos[1],det_pos[2],ch))
    
    # print("=====> finish _dij_move check")

    # # 进行 _safe_move的测试 
    # time.sleep(0.5)
    # print("=====> start _safe_move check")
    # while True:
    #     pos = PHYSICAL_MSG.query_position()
    #     ss = input("safe action>")
    #     if (ss=='Q' or ss=='q'):
    #         break
    #     for ch in ss:
    #         do_safe_action(ch,ep_chassis)
    #         pos = PHYSICAL_MSG.query_position()
    #         print("cur = ({:.3f},{:.3f}) deg = {:.3f}".
    #         format(pos[0],pos[1],pos[2]))
    
    # print("=====> finish _safe_move check")

    # pos = PHYSICAL_MSG.query_position()
    # print("init = ({:.3f},{:.3f}) deg = {:.3f}".
    #     format(pos[0],pos[1],pos[2]))

    # # 获取车的结束位置
    # pos = PHYSICAL_MSG.query_position()
    # print("finish = ({:.3f},{:.3f}) deg = {:.3f}".
    #     format(pos[0],pos[1],pos[2]))
    
    # 平台初始化完成
    print("=====> end physical_platform_initialize")
    print("start run?(y/n)")


# if __name__ == '__main__':
#     get_initialized_robot()
flip = -1
def do_action(action,ep_chassis,move_dis=0.5,rot_deg=45,mute=False):
    xy_speed=0.3
    z_speed=60
    if (action == 'W'):
        ep_chassis._dij_move(move_dis,0,0,xy_speed,z_speed).wait_for_completed()
    elif (action == 'S'):
        ep_chassis._dij_move(-move_dis,0,0,xy_speed,z_speed).wait_for_completed()
    elif (action == 'A'):
        ep_chassis._dij_move(0,-move_dis,0,xy_speed,z_speed).wait_for_completed()
    elif (action == 'D'):
        ep_chassis._dij_move(0,move_dis,0,xy_speed,z_speed).wait_for_completed()
    elif (action == 'L'):
        ep_chassis._dij_move(0,0,rot_deg,xy_speed,z_speed).wait_for_completed()
    elif (action == 'R'):
        ep_chassis._dij_move(0,0,-rot_deg,xy_speed,z_speed).wait_for_completed()
    elif (action == 'l'):
        ep_chassis._dij_move(0,0,10,xy_speed,z_speed).wait_for_completed()
    elif (action == 'r'):
        ep_chassis._dij_move(0,0,-10,xy_speed,z_speed).wait_for_completed()
    elif (action == 'w'):
        ep_chassis._dij_move(0.1,0,0,xy_speed,z_speed).wait_for_completed()
    elif (action == 'B'):
        global flip
        ep_chassis._dij_move(0,0,180*flip,xy_speed,z_speed).wait_for_completed()
        flip=-flip
    elif (action == 'F'):
        PHYSICAL_MSG.location_server_reset()
    else:
        pass

def do_safe_action(action,ep_chassis,mute=False):
    xy_speed=0.6
    z_speed=60
    move_dis = 1
    little_dis = 0.2
    rot_deg = 45
    if (action == 'W'):
        ep_chassis.move(move_dis,0,0,xy_speed,z_speed).wait_for_completed()
    elif (action == 'E'):
        ep_chassis.move(move_dis,move_dis,0,xy_speed,z_speed).wait_for_completed()
    elif (action == 'S'):
        ep_chassis.move(-move_dis,0,0,xy_speed,z_speed).wait_for_completed()
    elif (action == 'A'):
        ep_chassis.move(0,-move_dis,0,xy_speed,z_speed).wait_for_completed()
    elif (action == 'D'):
        ep_chassis.move(0,move_dis,0,xy_speed,z_speed).wait_for_completed()
    elif (action == 'L'):
        ep_chassis.move(0,0,rot_deg,xy_speed,z_speed).wait_for_completed()
    elif (action == 'R'):
        ep_chassis.move(0,0,-rot_deg,xy_speed,z_speed).wait_for_completed()
    elif (action == 'w'):
        ep_chassis.move(little_dis,0,0,xy_speed,z_speed).wait_for_completed()
    elif (action == 's'):
        ep_chassis.move(-little_dis,0,0,xy_speed,z_speed).wait_for_completed()
    elif (action == 'a'):
        ep_chassis.move(0,-little_dis,0,xy_speed,z_speed).wait_for_completed()
    elif (action == 'd'):
        ep_chassis.move(0,little_dis,0,xy_speed,z_speed).wait_for_completed()
    elif (action == 'l'):
        ep_chassis.move(0,0,10,xy_speed,z_speed).wait_for_completed()
    elif (action == 'r'):
        ep_chassis.move(0,0,-10,xy_speed,z_speed).wait_for_completed()
    elif (action == 'B'):
        ep_chassis.move(0,0,180,xy_speed,z_speed).wait_for_completed()
    elif (action == 'F'):
        PHYSICAL_MSG.location_server_reset()
    else:
        pass

def flush_undefined_behavior():
    # drive_speed(0,0,0) 会有UB，
    # 使用_dij_move(0,0,0).wait_for_completed刷新一下
    # 注意必须在底盘没有Action时，调用该函数。
    # 判断方法：报错 Robot is already performing action(s)
    # EP_ROBOT.chassis._dij_move(0,0,0).wait_for_completed()
    pass

def adjust(ep_chassis,is_manual=False):
    if not PLATFORM_TIMER_MANAGER.adjust_status_start():
        return 

    if is_manual:
        while True:
            ss = input("adjust_action>")
            if (ss=='Q' or ss=='q'):
                break
            for ch in ss:
                do_action(ch,ep_chassis)
    else:
        # step1  
        time.sleep(0.5)
        match_action = "SAWD"
        dis_dir = "FRBL"
        while(1):
            dis = PHYSICAL_MSG.get_sensor_data_info()[:]
            min_id = 0
            for i in range(4):
                if dis[i]<dis[min_id]: min_id=i

            if (dis[min_id]>300): break
            match_distance = 300 - dis[min_id]
            match_distance = max(100,match_distance)

            print(">>> adjust {} {} {}".format(dis_dir[min_id],
            dis[min_id],match_action[min_id]))
            do_action(match_action[min_id],ep_chassis,
            max(0.1,match_distance*0.001))

        # step2 
        do_action('B',ep_chassis)

    # stop adjust status

    PLATFORM_TIMER_MANAGER.adjust_status_end()

def encoder_proto(proto):
    s = "move,{:.3f},{:.3f},{:.3f},3,3".format(proto._x_spd, proto._y_spd, proto._z_spd)
    return s